package net.guruqu.simpleTest;

import java.awt.Canvas;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.image.BufferedImage;

import javax.swing.JFrame;

import net.guruqu.rvoSystem.RVOSimulator;
import net.guruqu.rvoSystem.Vector2;

public class SimpleVisualizor extends Canvas {
	RVOSimulator simulator;
	BufferedImage offImg;

	public RVOSimulator getSimulator() {
		return simulator;
	}

	public SimpleVisualizor(RVOSimulator simulator) {
		this.simulator = simulator;
	}

	public void step() {
		simulator.doStep();
		repaint();
	}

	@Override
	public void update(Graphics g) {
		paint(g);
	}

	@Override
	public void paint(Graphics g) {
		if (offImg == null || offImg.getWidth() != getWidth()
				|| offImg.getHeight() != getHeight()) {
			offImg = new BufferedImage(getWidth(), getHeight(),
					BufferedImage.TYPE_INT_ARGB);
		}
		Graphics2D g2d = offImg.createGraphics();
		g2d.setColor(Color.white);
		g2d.fillRect(0, 0, getWidth(), getHeight());

		g2d.translate(250, 250);
		g2d.setColor(Color.blue);
		int obstacles = simulator.getNumObstacles();
		for (int i = 0; i < obstacles; i++) {
			Vector2 v1 = simulator.getObstaclePoint1(i);
			Vector2 v2 = simulator.getObstaclePoint2(i);
			g2d
					.drawLine((int) v1.x(), (int) v1.y(), (int) v2.x(),
							(int) v2.y());
		}

		g2d.setColor(Color.black);
		int roadmap = simulator.getNumRoadmapVertices();
		Polygon s = new Polygon();
		for (int i = 0; i < obstacles; i++) {
			Vector2 v = simulator.getRoadmapVertexPosition(i);
			g2d.fillOval((int) v.x() - 2, (int) v.y() - 2, 5, 5);
		}

		g2d.setColor(Color.red);
		int agents = simulator.getNumAgents();
		for (int i = 0; i < agents; i++) {
			Vector2 pos = simulator.getAgentPosition(i);
			float ori = simulator.getAgentOrientation(i);
			g2d.fillRect((int) pos.x() - 2, (int) pos.y() - 2, 5, 5);
		}

		g.drawImage(offImg, 0, 0, this);
	}

	public static void setupScenario(RVOSimulator sim) {
		// Specify global time step of the simulation
		sim.setTimeStep(0.25f);

		// Specify default parameters for agents that are subsequently added
		sim
				.setAgentDefaults(250, 15.0f, 10, 2.0f, 3.0f, 1.0f, 2.0f, 7.5f,
						1.0f);

		// Add agents (and simulataneously their goals), specifying their
		// start position and goal ID
		sim.addAgent(new Vector2(-50.0f, -50.0f), sim.addGoal(new Vector2(
				50.0f, 50.0f)));
		sim.addAgent(new Vector2(50.0f, -50.0f), sim.addGoal(new Vector2(
				-50.0f, 50.0f)));
		sim.addAgent(new Vector2(50.0f, 50.0f), sim.addGoal(new Vector2(-50.0f,
				-50.0f)));
		sim.addAgent(new Vector2(-50.0f, 50.0f), sim.addGoal(new Vector2(50.0f,
				-50.0f)));

		// Add (line segment) obstacles, specifying both endpoints of the
		// line segments
//		sim.addObstacle(new Vector2(-7.0f, -20.0f), new Vector2(-7.0f, 20.0f));
//		sim.addObstacle(new Vector2(-7.0f, 20.0f), new Vector2(7.0f, 20.0f));
//		sim.addObstacle(new Vector2(7.0f, 20.0f), new Vector2(7.0f, -20.0f));
//		sim.addObstacle(new Vector2(7.0f, -20.0f), new Vector2(-7.0f, -20.0f));

//		sim.addObstacle(new Vector2(9.0f, -20.0f), new Vector2(9.0f, 20.0f));
//		sim.addObstacle(new Vector2(9.0f, 20.0f), new Vector2(17.0f, 20.0f));
//		sim.addObstacle(new Vector2(17.0f, 20.0f), new Vector2(17.0f, -20.0f));
//		sim.addObstacle(new Vector2(17.0f, -20.0f), new Vector2(9.0f, -20.0f));
//		
		// Add roadmap vertices, specifying their position
//		sim.addRoadmapVertex(new Vector2(-10.0f, -23.0f));
//		sim.addRoadmapVertex(new Vector2(-10.0f, 23.0f));
//		sim.addRoadmapVertex(new Vector2(10.0f, 23.0f));
//		sim.addRoadmapVertex(new Vector2(10.0f, -23.0f));

		// Do not automatically create edges between mutually visible
		// roadmap vertices
		sim.setRoadmapAutomatic(false);

		// Manually specify edges between vertices, specifying the ID's of
//		// the vertices the edges connect
//		sim.addRoadmapEdge(0, 1);
//		sim.addRoadmapEdge(1, 2);
//		sim.addRoadmapEdge(2, 3);
//		sim.addRoadmapEdge(3, 0);
	}

	public static void main(String s[]) throws InterruptedException {
		RVOSimulator simulator = new RVOSimulator();
		setupScenario(simulator);
		simulator.initSimulation();
		SimpleVisualizor simpleVisualizor = new SimpleVisualizor(simulator);
		simpleVisualizor.setVisible(true);
		simpleVisualizor.setSize(500, 500);

		JFrame frame = new JFrame();
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.getContentPane().add(simpleVisualizor);
		frame.pack();
		frame.setVisible(true);
		while (!simpleVisualizor.getSimulator().getReachedGoal()) {
			simpleVisualizor.step();
			Thread.sleep(10);
		}
	}
}
